﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.IO;
using System.Net.Sockets;
using System.Threading;
using System.Globalization;
// CCR & DSS Namespaces
using Microsoft.Ccr.Core;
using Microsoft.Ccr.Adapters.WinForms;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Dss.Core.DsspHttp;


namespace JaguarControl
{
    public partial class MainForm : Form
    {
        #region Variables
        private TcpClient clientSocketIMU = null;
        private Thread threadClientIMU = null;
        private NetworkStream cmdStreamIMU = null;


        private StreamReader readerIMU = null;
        private StreamWriter writerIMU = null;
        private delegate void updateSensorDataInfo(string data);

        private TcpClient clientSocketGPS = null;
        private Thread threadClientGPS = null;
        private NetworkStream cmdStreamGPS = null;
        private StreamReader readerGPS = null;


        //private bool firstDataIMU = true;
        //private bool firstDataGPS = true;

        private bool receivingGPS = false;
        private bool receivingIMU = false;



        //private int DATA_LEN = 14;      

        private int seqNo = 0;
        private double accel_x = 0;
        private double accel_y = 0;
        private double accel_z = 0;
        private double accel_x_offset = 0;
        private double accel_y_offset = 0;
        private double accel_z_offset = 0;

        private double gyro_x = 0;
        private double gyro_y = 0;
        private double gyro_z = 0;

        private double gyro_x_offset = 377;
        private double gyro_y_offset = 377;
        private double gyro_z_offset = 377;

        public struct IMURecord
        {
            public int index;
            public double accel_x;
            public double accel_y;
            public double accel_z;
            public double gyro_x;
            public double gyro_y;
            public double gyro_z;
            public double magn_x;
            public double magn_y;
            public double magn_z;
            public double eRoll;
            public double ePitch;
            public double eYaw;
        }

        public IMURecord imuData = new IMURecord();
        public IMURecord imuRecord = new IMURecord();
        public int recordCnt = 0;
        public bool startRecord = false;
        public StreamWriter SW;
        public string fileNme = "c:\\DrRobotAppFile\\DrRobotGPSIMURec";
        public int fileCnt = 0;
        public const int MAXFILELEN = 32767;
        public struct GPSRecord
        {
            public double latitude;
            public double longitude;
            public double altitude;
            public double seaHeight;
            public double geoidalHeight;
            public double vog;
            public double cog;
            public int satNum;
            public double timeStamp;
            public string latHemi;
            public string lngHemi;
            public double h_pre;
            public double v_pre;
            public double p_pre;
            public string status;
            public int qi;
            public string recRaw;
        }
        public GPSRecord gpsData = new GPSRecord();
        public GPSRecord gpsRecord = new GPSRecord();
        public const double KNNOT2MS = 0.514444444;
        private delegate void updateGPSSensorDataInfo(string data);

        private const int DrawDataLen = 200;        //around 4 second
        private double[] draw_AccelX = new double[DrawDataLen];
        private double[] draw_AccelY = new double[DrawDataLen];
        private double[] draw_AccelZ = new double[DrawDataLen];
        private double[] draw_GyroX = new double[DrawDataLen];
        private double[] draw_GyroY = new double[DrawDataLen];
        private double[] draw_GyroZ = new double[DrawDataLen];
        private int drawStartPoint = 0;
        private int drawEndPoint = 0;
        #endregion

        //HERE ARE THE THREADS !
        private void startComm()
        {
            int remotePortGPS = jaguarSetting.GPSPort;
            String IPAddrGPS = jaguarSetting.GPSIP;
            try
            {
                clientSocketGPS = new TcpClient();
                IAsyncResult result = clientSocketGPS.BeginConnect(IPAddrGPS, remotePortGPS, null, null);
                bool success = result.AsyncWaitHandle.WaitOne(500, true);
                if (!success)
                {
                    receivingGPS = false;
                }
                else
                {
                    //firstDataGPS = true;
                    receivingGPS = true;
                    threadClientGPS = new Thread(new ThreadStart(HandleClientGPS));
                    threadClientGPS.CurrentCulture = new CultureInfo("en-US");
                    threadClientGPS.Start();

                }
            }
            catch
            {
                receivingGPS = false;

            }


            //for IMU
            int remotePort = jaguarSetting.IMUPort;
            String IPAddr = jaguarSetting.IMUIP;
            try
            {
                clientSocketIMU = new TcpClient();
                IAsyncResult result = clientSocketIMU.BeginConnect(IPAddr, remotePort, null, null);
                bool success = result.AsyncWaitHandle.WaitOne(500, true);
                if (!success)
                {
                    receivingIMU = false;
                    Console.WriteLine("IMUupdate" + System.DateTime.Now.ToString());
                }
                else
                {
                    //firstDataIMU = true;
                    receivingIMU = true;
                    threadClientIMU = new Thread(new ThreadStart(HandleClientIMU));
                    threadClientIMU.CurrentCulture = new CultureInfo("en-US");
                    threadClientIMU.Start();
                }
            }
            catch
            {
                receivingIMU = false;
            }


        }
        private void stopCommunication()
        {

            if (clientSocketIMU != null)
                clientSocketIMU.Close();

            receivingIMU = false;
            if (writerIMU != null)
            {
                try
                {
                    writerIMU.Close();
                }
                catch
                {
                }
            }
            if (readerIMU != null)
                readerIMU.Close();
            if (cmdStreamIMU != null)
                cmdStreamIMU.Close();
            if (threadClientIMU != null)
                threadClientIMU.Abort();


            if (clientSocketGPS != null)
                clientSocketGPS.Close();

            receivingGPS = false;

            if (readerGPS != null)
                readerGPS.Close();
            if (cmdStreamGPS != null)
                cmdStreamGPS.Close();
            if (threadClientGPS != null)
                threadClientGPS.Abort();
        }

        #region IMU
        private void HandleClientIMU()
        {
            try
            {
                //here process all receive data from client
                cmdStreamIMU = clientSocketIMU.GetStream();
                readerIMU = new StreamReader(cmdStreamIMU);
                writerIMU = new StreamWriter(cmdStreamIMU);


                string strRec = "";

                while (receivingIMU)
                {
                    try
                    {

                        if (!cmdStreamIMU.DataAvailable)      //there is no data
                        {
                            //need to sleep for other thread
                            Thread.Sleep(5);        //50 Hz
                        }
                        else
                        {
                            // Reads NetworkStream into a byte buffer.
                            strRec = readerIMU.ReadLine();
                            if (strRec != null)
                                processDataIMU(strRec);



                        }
                    }
                    catch (Exception e)
                    {
                        string temp = e.ToString();

                    }
                    finally
                    {
                    }


                }
                receivingIMU = false;

                readerIMU.Close();
                cmdStreamIMU.Close();
                clientSocketIMU.Close();
            }
            catch
            {
                receivingIMU = false;
            }


        }
        private void processDataIMU(string msg)
        {
            string[] data = msg.Split(',');

            if (data.Length == 14) //the whole package here
            {
                Invoke(new updateSensorDataInfo(updateSensorIMU), msg);
                seqNo++;
                accel_x = double.Parse(data[8]);
                accel_y = double.Parse(data[9]);
                accel_z = double.Parse(data[10]);
                gyro_x = double.Parse(data[7]);
                gyro_y = double.Parse(data[5]);
                gyro_z = double.Parse(data[6]);


                if (calibrateIMU)
                {
                    gyro_x_sum += gyro_x;
                    gyro_y_sum += gyro_y;
                    gyro_z_sum += gyro_z;
                    
                    calibrateCnt++;
                    if (calibrateCnt >= CALIBRATION_NUM)
                    {
                        calibrateIMU = false;
                        calibrateCnt = CALIBRATION_NUM;

                        gyro_x_offset = gyro_x_sum / CALIBRATION_NUM;
                        gyro_y_offset = gyro_y_sum / CALIBRATION_NUM;
                        gyro_z_offset = gyro_z_sum / CALIBRATION_NUM;
                    }

                }
                else
                {
                    gyro_x -= gyro_x_offset;
                    gyro_y -= gyro_y_offset;
                    gyro_z -= gyro_z_offset;
                }

                if (!calibrateIMU)
                {
                    imuRecord.accel_x = accel_x;
                    imuRecord.accel_y = accel_y;
                    imuRecord.accel_z = accel_z;
                    imuRecord.gyro_x = gyro_x;
                    imuRecord.gyro_y = gyro_y;
                    imuRecord.gyro_z = gyro_z;
                    imuRecord.index = seqNo;
                    imuRecord.magn_x = double.Parse(data[11]);
                    imuRecord.magn_y = double.Parse(data[12]);
                    imuRecord.magn_z = double.Parse(data[13]);
                    imuRecord.eRoll = double.Parse(data[1]);
                    imuRecord.ePitch = double.Parse(data[2]);
                    imuRecord.eYaw = double.Parse(data[3]);
                    //for drawing
                    draw_AccelX[drawEndPoint] = accel_x - accel_x_offset;
                    draw_AccelY[drawEndPoint] = accel_y - accel_y_offset;
                    draw_AccelZ[drawEndPoint] = accel_z - accel_z_offset;
                    draw_GyroX[drawEndPoint] = gyro_x - gyro_x_offset;
                    draw_GyroY[drawEndPoint] = gyro_y - gyro_y_offset;
                    draw_GyroZ[drawEndPoint] = gyro_z - gyro_z_offset;
                    drawEndPoint++;
                    if (drawEndPoint >= DrawDataLen)
                    {
                        drawEndPoint = 0;

                    }

                    if (drawEndPoint == drawStartPoint)
                    {
                        drawStartPoint++;
                        if (drawStartPoint >= DrawDataLen)
                            drawStartPoint = 0;
                    }

                    if (startRecord)
                    {
                        String recTemp = imuRecord.index.ToString() + "," +
                                        imuRecord.accel_x.ToString() + "," +
                                        imuRecord.accel_y.ToString() + "," +
                                        imuRecord.accel_z.ToString() + "," +
                                        imuRecord.gyro_x.ToString() + "," +
                                        imuRecord.gyro_y.ToString() + "," +
                                        imuRecord.gyro_z.ToString() + "," +
                                        imuRecord.magn_x.ToString() + "," +
                                        imuRecord.magn_y.ToString() + "," +
                                        imuRecord.magn_z.ToString() + "," +
                                        imuRecord.ePitch.ToString() + "," +
                                        imuRecord.eRoll.ToString() + "," +
                                        imuRecord.eYaw.ToString();

                        SW.WriteLine(recTemp);
                        recordCnt++;
                        if (recordCnt > MAXFILELEN)
                        {
                            recordCnt = 0;
                            SW.Close();
                            //open next file

                            SW = File.CreateText(fileNme + fileCnt.ToString() + ".txt");
                            fileCnt++;

                        }
                    }
                }

            }

            
        }
        private void updateSensorIMU(string msg)
        {
            //textBoxRCV.Text = msg; Was used for displaying the complete IMU Raw Data
            ///<Elements in msg can be accessed by using string[] data = msg.Split(',');
            ///<The elements are: 
            ///<data[1] Roll
            ///<data[2] Pitch
            ///<data[3] Yaw
            ///<data[4] 
            ///<data[5] Gyro Y
            ///<data[6] Gyro Z
            ///<data[7] Gyro X
            ///<data[8] Accel X
            ///<data[9] Accel Y
            ///<data[10] Accel Z
            ///<data[11] Magn X
            ///<data[12] Magn Y
            ///<data[13] Magn Z

            
            //This variable is just for testing purposes without modifying the original record variable.
            //Negatives are for reading CCW values.
            imuData = imuRecord;
            imuData.eYaw = -imuData.eYaw;
            //imuData.ePitch = -imuData.ePitch;
            //imuData.eRoll = -imuData.eRoll;
            textboxPitch.Text = imuData.ePitch.ToString();
            textboxRoll.Text = imuData.eRoll.ToString();
            textboxYaw.Text = imuData.eYaw.ToString();
            
        }
        #endregion 

        #region GPS
        private void HandleClientGPS()
        {
            //here process all receive data from client
            try
            {
                cmdStreamGPS = clientSocketGPS.GetStream();
                readerGPS = new StreamReader(cmdStreamGPS);

                string strRec = "";

                while (receivingGPS)
                {
                    try
                    {

                        if (!cmdStreamGPS.DataAvailable)      //there is no data
                        {
                            //need to sleep for other thread
                            Thread.Sleep(40);        //5 Hz
                        }
                        else
                        {
                            // Reads NetworkStream into a byte buffer.
                            strRec = readerGPS.ReadLine();
                            if (strRec != null)
                                processDataGPS(strRec);
                        }
                    }
                    catch (Exception e)
                    {
                        string temp = e.ToString();

                    }
                    finally
                    {
                    }


                }
                receivingGPS = false;
                readerGPS.Close();
                cmdStreamGPS.Close();
                clientSocketGPS.Close();
            }
            catch
            {
                receivingGPS = false;
            }

        }
        private void updateSensorGPS(string rcv)
        {

            //textBoxGPSRCV.Text = rcv;  Was used for displaying the complete GPS Raw Data
            lblCOG.Text = gpsRecord.cog.ToString();
            lblVOG.Text = gpsRecord.vog.ToString("0.00");
            lblLat.Text = gpsRecord.latitude.ToString();
            lblLong.Text = gpsRecord.longitude.ToString();

            if (gpsRecord.qi == 0)
            {
                lblGPSQI.Text = "Fix not avaiable";
            }
            else if (gpsRecord.qi == 1)
            {
                lblGPSQI.Text = "Non DGPS fix avaiable";
            }
            else if (gpsRecord.qi == 2)
            {
                lblGPSQI.Text = "DGPS fix avaiable";
            }
            else if (gpsRecord.qi == 6)
            {
                lblGPSQI.Text = "Estimate";
            }
            else
            {
                lblGPSQI.Text = "Fix not avaiable";
            }
            
        }
        private void processDataGPS(string input)
        {

            if ((input.Substring(0, 3) == "$GP") || (input.Substring(0, 3) == "$PG"))
            {
                gpsRecord.recRaw = input;
                Invoke(new updateGPSSensorDataInfo(updateSensorGPS), input);
                if (startRecord)
                {
                    SW.WriteLine(gpsRecord.recRaw);
                    recordCnt++;
                    if (recordCnt > MAXFILELEN)
                    {
                        recordCnt = 0;
                        SW.Close();
                        //open next file

                        SW = File.CreateText(fileNme + fileCnt.ToString() + ".txt");
                        fileCnt++;

                    }
                }
            }

            String[] strData = input.Split(',');
            double dirLat = 1;
            double dirLongitude = -1;
            double longitude = 0;
            double lat = 0;
            if ((strData[0] == "$GPRMC") && (strData[3] != "") && (strData[5] != ""))
            {
                gpsRecord.timeStamp = double.Parse(strData[1]);     //time
                gpsRecord.status = strData[2];                        //A- valid, V = invalid
                gpsRecord.latitude = double.Parse(strData[3]);
                gpsRecord.latHemi = strData[4];
                gpsRecord.longitude = double.Parse(strData[5]);   //dddmm.mmmm
                gpsRecord.lngHemi = strData[6];
                gpsRecord.vog = double.Parse(strData[7]) * KNNOT2MS;
                gpsRecord.cog = double.Parse(strData[8]);


                if (gpsRecord.latHemi == "N")
                    dirLat = 1;
                else
                    dirLat = -1;

                if (gpsRecord.lngHemi == "W")
                    dirLongitude = -1;
                else
                    dirLongitude = 1;


                curLatitude = dirLat * ToDegree(gpsRecord.latitude);
                curLongitude = dirLongitude * ToDegree(gpsRecord.longitude);

                if ((preLatitude == 0) && (preLongtitude == 0))
                {
                    preEstLatitude = lat;
                    preEstLongitude = longitude;

                    preLatitude = curLatitude;
                    preLongtitude = curLongitude;
                }
                else
                {
                            //display real time GPS in satellital viewer
                            if ((preLatitude != 0) && (preLongtitude != 0))
                            {
                                gps_displayCnt++;
                                if (gps_displayCnt >= GPS_DISPER)
                                {
                                    gps_t++;
                                    if (gps_t > 300)
                                        gps_t = 0;

                                    gps_displayCnt = 0;
                                    kmlLineDataMakeLoad(setColor.SetBlue, 4, preLatitude, preLongtitude, curLatitude, curLongitude);
                                    preEstLatitude = lat;
                                    preEstLongitude = longitude;

                                    preLatitude = curLatitude;
                                    preLongtitude = curLongitude;
                                }
                            }
                }



                ////DCM
                //if ((strData[7] != "") && strData[8] != null)
                //{
                //    vog = double.Parse(strData[7]) * KNNOT2MS; //speed
                //    cog = double.Parse(strData[8]) * D2R; //course over ground
                //    if (cog > Math.PI) cog = cog - 2 * Math.PI;

                //    if (!disableGPS)
                //    {
                //        unsafe
                //        {
                //            fixed (double* estPosP = estPos)
                //                UpdateGPS(cog, vog);
                //        }
                //    }
                //    gpsRecord.vog = double.Parse(strData[7]) * KNNOT2MS;
                //    gpsRecord.cog = double.Parse(strData[8]);
                //}

            }
            else if (strData[0] == "$GPGGA")
            {
                gpsRecord.timeStamp = double.Parse(strData[1]);     //time
                gpsRecord.latitude = double.Parse(strData[2]);
                gpsRecord.latHemi = strData[3];
                gpsRecord.longitude = double.Parse(strData[4]);   //dddmm.mmmm
                gpsRecord.lngHemi = strData[5];
                gpsRecord.qi = int.Parse(strData[6]);
                gpsRecord.satNum = int.Parse(strData[7]);
                gpsRecord.h_pre = double.Parse(strData[8]);
                gpsRecord.seaHeight = double.Parse(strData[9]);
                gpsRecord.geoidalHeight = double.Parse(strData[10]);
                curLatitude = ToDegree(gpsRecord.latitude);
                curLongitude = -ToDegree(gpsRecord.longitude);

            }
            else if (strData[0] == "$GPGSA")
            {

                gpsRecord.p_pre = double.Parse(strData[4]);
                gpsRecord.h_pre = double.Parse(strData[5]);
                gpsRecord.v_pre = double.Parse(strData[6]);

            }
            else if (strData[0] == "$GPGSV")
            {
                int len = 0;
                len = strData.Length;
                gpsRecord.satNum = int.Parse(strData[3]);
            }

            //This variable is just for testing purposes without modifying the original record variable.
            gpsData = gpsRecord;
        }
        #endregion

        #region Helpers
        private double ToDegree(double angle)
        {
            double deg = 0;

            int degree = (int)(angle / 100);
            double min = ((angle - degree * 100));

            deg = degree + ((double)min) / 60;

            return deg;
        }
        private void kmlLineDataMakeLoad(string settingColor, int setWidth, double preLatitude, double preLongtitude, double lat, double longitude)
        {
            //make data
            kmlFileCoordinate = @"<coordinates>" + preLongtitude.ToString() + "," + preLatitude.ToString() + "," + "0" +
            " " + longitude.ToString() + "," + lat.ToString() + "," + "0</coordinates>";
            kmlFileStyle = @"<Style id=""MyLineStyle""><LineStyle><color>" + settingColor + @"</color><width>" + setWidth.ToString() + @"</width></LineStyle></Style>";
            string data = kmlFileStr1 + kmlFileStyle + kmlFilePlacemark + kmlFileCoordinate + kmlFileEnd;
            googleEarth.LoadKmlData(ref data);

        }
        #endregion
    }
}
